docs/development/PID Internals.md

   1 ### IO variables
   2
   3 `gyroADC/8192*2000 = deg/s`
   4
   5 `gyroADC/4 ~ deg/s`
   6
   7 `rcCommand` - `<-500 - 500>` nominal, but is scaled with `rcRate/100`, max +-1250
   8
   9 `inclination` - in 0.1 degree, roll and pitch deviation from horizontal position
  10 `max_angle_inclination` - in 0.1 degree, default 50 degrees (500)
  11
  12 `axisPID` - output to mixer, will be added to throttle(`<1000-2000>`), output range is `<minthrottle, maxthrottle>` (default `<1150 - 1850>`)
  13
  14 ### PID controller 0, "MultiWii" (default)
  15
  16
  17 #### Leveling term
  18 ```
  19 error = constrain(2*rcCommand[axis], limit +- max_angle_inclination) - inclination[axis]
  20 Pacc = constrain(P8[PIDLEVEL]/100 * error, limit +- 5 * D8[PIDLEVEL])
  21 Iacc = intergrate(error, limit +-10000) * I8[PIDLEVEL] / 4096
  22 ```
  23 #### Gyro term
  24 ```
  25 Pgyro = rcCommand[axis];
  26 error = rcCommand[axis] * 10 * 8 / pidProfile->P8[axis] - gyroADC[axis] / 4; (conversion so that error is in deg/s ?)
  27 Igyro = integrate(error, limit +-16000) / 10 / 8  * I8[axis] / 100 (conversion back to mixer units ?)
  28 ```
  29
  30 reset I term if
  31   - axis rotation rate > +-64deg/s
  32   - axis is YAW and rcCommand>+-100
  33
  34 ##### Mode dependent mix(yaw is always from gyro)
  35
  36 - HORIZON - proportionally according to max deflection
  37 ```
  38   deflection = MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])) / 500 ; limit to 0.0 .. 1.0
  39   P = Pacc * (1-deflection) + Pgyro * deflection
  40   I = Iacc * (1-deflection) + Igyro * deflection
  41 ```
  42 - gyro
  43 ```
  44   P = Pgyro
  45   I = Igyro
  46 ```
  47 - ANGLE
  48 ```
  49   P = Pacc
  50   I = Iacc
  51 ```
  52 #### Gyro stabilization
  53
  54 ```
  55 P -=  gyroADC[axis] / 4 * dynP8 / 10 / 8
  56 D = -mean(diff(gyroADC[axis] / 4), over 3 samples) * 3 * dynD8 / 32
  57 [equivalent to :]
  58 D = - (gyroADC[axis]/4 - (<3 loops old>gyroADC[axis]/4)) * dynD8 / 32
  59 ```
  60
  61 This can be seen as sum of
  62  - PI controller (handles rcCommand, HORIZON/ANGLE); `Igyro` is only output based on gyroADC
  63  - PD controller(parameters dynP8/dynD8) with zero setpoint acting on gyroADC